/*******************************************************************************
 * $Id: main.c 16 2012-02-08 01:27:53Z reviakinea@gmail.com $
 ******************************************************************************/

#include <inttypes.h>
#include <stdlib.h>

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>

#include "uart/uart.h"

// UART baudrate
#define		UART_BAUDRATE	9600

// one quarter of the circle
#define		MAX_THETA		120		
#define		MAX_THETAx2		240
#define		MAX_THETAx3		360
#define		MAX_THETAx4		480

// 64MHz (PLL frequency) / 2666 / 2 = 12 kHz (PWM frequency)
#define		MAX_PWM			2666

// used for the angle integrator
#define		K_scal			16		

volatile uint8_t	Flag_IT_timer0=0, Flag_IT_ADC=0 ;
uint16_t	counter = 0 ;

int16_t		Omega_meas;
int16_t		Omega_ref = 100; // Omega_ref = w * 1.223 = n * 0.128
int16_t		Command = 0;
uint16_t	theta1=0, theta2=160, theta3=320 ;

int16_t		amplitude , OmegaTe = 64 ;
uint8_t		direction = 0 ;
uint16_t    PWM0, PWM1, PWM2, DeadTime = 254 ;   // 254 => temps mort = 4 µs

char		buffer[16];

void		init(void);
void		PSC_Init(unsigned int ot0,  unsigned int ot1);
void		PSC0_Load (unsigned int dt0, unsigned int dt1);
void		PSC1_Load (unsigned int dt0, unsigned int dt1);
void		PSC2_Load (unsigned int dt0, unsigned int dt1);
uint16_t	controlVF(uint16_t wTs);
uint16_t	duty_cycle(uint16_t theta, uint16_t Vm) ;

void main(void)
{	
	init();
	PSC_Init(0x00, MAX_PWM); 
	uart_init(UART_BAUD_SELECT_DOUBLE_SPEED(UART_BAUDRATE, F_CPU));
	
	while(1)
	{
		if (Flag_IT_timer0)
		{
			uint16_t ch = uart_getc();
			if (!(	(ch & UART_NO_DATA)		||
					(ch & UART_FRAME_ERROR) || 
					(ch & UART_OVERRUN_ERROR)
				))
			{
				switch((uint8_t)ch)
				{
				case 'r':					
					uart_puts_P("Omega_ref = ");
					itoa(Omega_ref, buffer, 10);
					uart_puts(buffer);
					uart_puts_P("\r\n");
					break;
					
				default:
					uart_puts_P("Unknown command: ");
					uart_putc((uint8_t)ch);
					uart_puts_P("\r\n");
					break;
				}
			}
			
			// get the measured speed from the ADC
			//Omega_meas = read_acquisition();
			
			// compute the stator frequency (PI controller)
			// Command = mc_control_speed_16b(Omega_ref,Omega_meas);
			
			// for use in open loop
			Command = Omega_ref;	
			
			// direction management : extract sign and absolute value
			if (Command > (int16_t)(0))
			{
				direction = 0;
				OmegaTe   = Command;
			} else {
				direction = 1;
				OmegaTe   = (~Command) + 1;
			}
			
			if (OmegaTe > K_scal)
			{
			// ------------------------ V/f law --------------------------
				amplitude = controlVF(OmegaTe);
				
			// ------------------ natural PWN algorithm ------------------
				PWM0 = duty_cycle(theta1,amplitude);
				PWM1 = duty_cycle(theta2,amplitude);
				PWM2 = duty_cycle(theta3,amplitude);
			} else {
				PWM0 = 0;
				PWM1 = 0;
				PWM2 = 0;
			}
			
			// -------- load the PSCs with the new duty cycles -----------
			PSC0_Load(PWM0, PWM0 + DeadTime);
			if (!direction)
			{
				PSC1_Load(PWM1, PWM1 + DeadTime);
				PSC2_Load(PWM2, PWM2 + DeadTime);
			} else {
				PSC2_Load(PWM1, PWM1 + DeadTime);
				PSC1_Load(PWM2, PWM2 + DeadTime);
			}
			
			// 3 integrators for the evolution of the angles
			theta1 = (K_scal * theta1 + OmegaTe) / K_scal; 
			theta2 = theta1 + 160;
			theta3 = theta1 + 320;
			
			if (theta1>=MAX_THETAx4) theta1 -= MAX_THETAx4;
			if (theta2>=MAX_THETAx4) theta2 -= MAX_THETAx4;
			if (theta3>=MAX_THETAx4) theta3 -= MAX_THETAx4;
			
			Flag_IT_timer0 = 0;
		}
	}
}

// interrupt vector for the sampling period (Ts=1 ms)
ISR(TIMER0_COMP_A_vect)
{
	Flag_IT_timer0 = 1;
}

// interrupt vector for the ADC (end of conversion)
/*ISR(ADC_vect)
{
	Flag_IT_ADC = 1;
}*/
